//
// Created by vivi on 24/03/2018.
//

#pragma once
#include "VPly/vply.h"
#include "core/collision/collisionobj.h"
#include "core/util/eigenutil.h"
#include "core/constant/constants.h"

namespace HairEngine {
	class SphereCollisionObject: public CollisionObject {
	Public:
		SphereCollisionObject(const Eigen::Vector3f & center_, float radius_,
		                      const Eigen::Affine3f & transform= Eigen::Affine3f::Identity()):
			CollisionObject(transform),
			center(center_),
			radius(radius_) {}

		const Eigen::Vector3f & getCenter() const {
			return center;
		}

		float getRadius() const {
			return radius;
		}

		float modelDistance(const Eigen::Vector3f & modelPos, Eigen::Vector3f *outGradientPtr = nullptr) const override {
			Eigen::Vector3f v = modelPos - center;
			if (outGradientPtr)
				*outGradientPtr = v;
			return v.norm() - radius;
		}

		std::ostream & vplyStream(std::ostream & os) const override {
			const std::string sep = " ";

			VPly::writeSphere(
					os,
					EigenUtility::toVPlyVector3f(center),
					radius,
					EigenUtility::toVPlyMatrix4f(localToWorldTransform.matrix()),
					VPly::VPlyIntAttr("type", VPlyOptions::CollisionObject_)
			);

			return os;
		}

		Eigen::AlignedBox3f boundingBox() const override {
			auto ret = Eigen::AlignedBox3f(center - Eigen::Vector3f(radius, radius, radius),
			                               center + Eigen::Vector3f(radius, radius, radius));
			return ret;
		}

	Protected:
		Eigen::Vector3f center;
		float radius;
	};
}